#include "pid.h"
#include "math_plus.h"

void local_pid(struct PID_parameters *self,float target,float current)
{
    self -> last_error = self -> error;
    self -> error = target - current;
    self -> i_error += self -> error;
    if (self->run_flag == first)
    {
        self -> last_error = self -> error;
        self->run_flag = other;
    }

    self->pid_sum   = MINMAX(self->kp * self -> error  ,-self -> max_kp_product,self -> max_kp_product)
                    + MINMAX(self->ki * self -> i_error   ,-self -> max_ki_product,self -> max_ki_product)
                    + MINMAX(self->kd * (self -> error -  self ->last_error),-self -> max_kd_product,self -> max_kd_product);

    self->pid_sum = MINMAX(self -> pid_sum,-self->max_pid_sum, self->max_pid_sum);

}

void inc_pid(struct PID_parameters *self,float target,float current)
{
    self -> last_2_error = self -> last_error;
    self -> last_error = self -> error;
    self -> error = target - current;
    if (self->run_flag == first)
    {
        self -> last_error = self -> error;
        self -> last_2_error = self -> last_error;
        self->run_flag = other;
    }

    self->pid_sum   += MINMAX(self->kp * (self -> error -self -> last_error),-self -> max_kp_product,self -> max_kp_product)
                    + MINMAX(self->ki * self -> error   ,-self -> max_ki_product,self -> max_ki_product)
                    + MINMAX(self->kd * (self -> error - 2*self->last_error + self->last_2_error),-self -> max_kd_product,self -> max_kd_product);

    self->pid_sum = MINMAX(self -> pid_sum,-self->max_pid_sum, self->max_pid_sum);
}